/******************** (C) COPYRIGHT 2014 ANO Tech ********************************
 * ����   �������ƴ�
 * �ļ���  ��data_transfer.c
 * ����    �����ݴ���
 * ����    ��www.anotc.com
 * �Ա�    ��anotc.taobao.com
 * ����QȺ ��190169595
 **********************************************************************************/
#include "zino.h"
#include "ANO_DT.h"
#include "rtthread.h"
#include "zino_imu.h"
#include "gyro.h"
#include "acceleration.h"
#include "rc.h"
#include <stdint.h>

#define DBG_TAG "ANO"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define ANO_DT_USE_CONSOLE
/////////////////////////////////////////////////////////////////////////////////////
// ���ݲ�ֺ궨�壬�ڷ��ʹ���1�ֽڵ���������ʱ������int16��float�ȣ���Ҫ�����ݲ�ֳɵ����ֽڽ��з���
#define BYTE0(dwTemp) (*((char *)(&dwTemp)))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

dt_flag_t f;              // ��Ҫ�������ݵı�־
uint8_t data_to_send[50]; // �������ݻ���
bool ant_run = false;
/////////////////////////////////////////////////////////////////////////////////////
extern zino_imu_t zino_imu;
extern struct acc_device zAccDev;
extern struct gyro_device zGyroDev;
/////////////////////////////////////////////////////////////////////////////////////
// Data_Exchange���������������ݷ������󣬱�����ʵ��ÿ5ms����һ�δ�������������λ�������ڴ˺�����ʵ��
// �˺���Ӧ���û�ÿ1ms����һ��
void ANO_DT_Data_Exchange(void)
{
    static uint8_t cnt = 0;
    static uint8_t senser_cnt = 10;
    static uint8_t status_cnt = 15;
    static uint8_t rcdata_cnt = 20;
    static uint8_t motopwm_cnt = 20;
    static uint8_t power_cnt = 50;

    if ((cnt % senser_cnt) == (senser_cnt - 1))
        f.send_senser = 1;

    if ((cnt % status_cnt) == (status_cnt - 1))
        f.send_status = 1;

    if ((cnt % rcdata_cnt) == (rcdata_cnt - 1))
        f.send_rcdata = 1;

    if ((cnt % motopwm_cnt) == (motopwm_cnt - 1))
        f.send_motopwm = 1;

    if ((cnt % power_cnt) == (power_cnt - 1))
        f.send_power = 1;

    cnt++;
    /////////////////////////////////////////////////////////////////////////////////////
    if (f.send_version)
    {
        f.send_version = 0;
        ANO_DT_Send_Version(4, 300, 100, 400, 0);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_status)
    {
        f.send_status = 0;
        // ANO_DT_Send_Status(Roll, Pitch, Yaw, baroAlt, 0, fly_ready);
        ANO_DT_Send_Status(
            (float)zino_imu.attitudes.roll*0.1f,
            (float)zino_imu.attitudes.pitch*0.1f,
            (float)zino_imu.attitudes.yaw*0.1f, 0, 0, 0);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_senser)
    {
        f.send_senser = 0;
        // ANO_DT_Send_Senser(mpu6050.Acc.x, mpu6050.Acc.y, mpu6050.Acc.z,
        //                    mpu6050.Gyro.x, mpu6050.Gyro.y, mpu6050.Gyro.z,
        //                    ak8975.Mag_Adc.x, ak8975.Mag_Adc.y, ak8975.Mag_Adc.z, 0);
        ANO_DT_Send_Senser(zAccDev.adc.x, zAccDev.adc.y, zAccDev.adc.z,
                           zGyroDev.adc.x, zGyroDev.adc.y, zGyroDev.adc.z,
                           0, 0, 0, 0);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_rcdata)
    {
        f.send_rcdata = 0;
        ANO_DT_Send_RCData(zinoCommonRcData.values.stick.LUD, zinoCommonRcData.values.stick.LLR,zinoCommonRcData.values.stick.RLR,zinoCommonRcData.values.stick.RUD,
        zinoCommonRcData.values.button.LU * 100+zinoCommonRcData.values.button.LD*200,
        zinoCommonRcData.values.button.LL * 100+zinoCommonRcData.values.button.LR*200,
        zinoCommonRcData.values.button.RU * 100+zinoCommonRcData.values.button.RD*200,
        zinoCommonRcData.values.button.RL * 100+zinoCommonRcData.values.button.RR*200,
        zinoCommonRcData.values.button.b*100+zinoCommonRcData.values.button.p*200+zinoCommonRcData.values.button.q*300+zinoCommonRcData.values.button.d*400,
        zinoCommonRcData.values.button.RT*100+zinoCommonRcData.values.button.LT*200
        );
        // ANO_DT_Send_RCData(Rc_Pwm_In[0], Rc_Pwm_In[1], Rc_Pwm_In[2], Rc_Pwm_In[3], Rc_Pwm_In[4], Rc_Pwm_In[5], Rc_Pwm_In[6], Rc_Pwm_In[7], 0, 0);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_motopwm)
    {
        f.send_motopwm = 0;
        ANO_DT_Send_MotoPWM(1, 2, 3, 4, 5, 6, 7, 8);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_power)
    {
        f.send_power = 0;
        ANO_DT_Send_Power(123, 456);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_pid1)
    {
        f.send_pid1 = 0;
        // ANO_DT_Send_PID(1, ctrl_1.PID[PIDROLL].kp, ctrl_1.PID[PIDROLL].ki, ctrl_1.PID[PIDROLL].kd,
        //                 ctrl_1.PID[PIDPITCH].kp, ctrl_1.PID[PIDPITCH].ki, ctrl_1.PID[PIDPITCH].kd,
        //                 ctrl_1.PID[PIDYAW].kp, ctrl_1.PID[PIDYAW].ki, ctrl_1.PID[PIDYAW].kd);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_pid2)
    {
        f.send_pid2 = 0;
        // ANO_DT_Send_PID(2, ctrl_1.PID[PID4].kp, ctrl_1.PID[PID4].ki, ctrl_1.PID[PID4].kd,
        //                 ctrl_1.PID[PID5].kp, ctrl_1.PID[PID5].ki, ctrl_1.PID[PID5].kd,
        //                 ctrl_1.PID[PID6].kp, ctrl_1.PID[PID6].ki, ctrl_1.PID[PID6].kd);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    else if (f.send_pid3)
    {
        f.send_pid3 = 0;
        // ANO_DT_Send_PID(3, ctrl_2.PID[PIDROLL].kp, ctrl_2.PID[PIDROLL].ki, ctrl_2.PID[PIDROLL].kd,
        //                 ctrl_2.PID[PIDPITCH].kp, ctrl_2.PID[PIDPITCH].ki, ctrl_2.PID[PIDPITCH].kd,
        //                 ctrl_2.PID[PIDYAW].kp, ctrl_2.PID[PIDYAW].ki, ctrl_2.PID[PIDYAW].kd);
    }
    /////////////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////////////
    // Usb_Hid_Send();
    /////////////////////////////////////////////////////////////////////////////////////
}

/////////////////////////////////////////////////////////////////////////////////////
// Send_Data������Э�������з������ݹ���ʹ�õ��ķ��ͺ���
// ��ֲʱ���û�Ӧ��������Ӧ�õ����������ʹ�õ�ͨ�ŷ�ʽ��ʵ�ִ˺���
void ANO_DT_Send_Data(uint8_t *dataToSend, uint8_t length)
{
#ifdef ANO_DT_USE_USB_HID
    Usb_Hid_Adddata(data_to_send, length);
#endif
#ifdef ANO_DT_USE_USART2
    Usart2_Send(data_to_send, length);
#endif

#ifdef ANO_DT_USE_CONSOLE
    rt_kput(data_to_send, length);
#endif
}

static void ANO_DT_Send_Check(uint8_t head, uint8_t check_sum)
{
    data_to_send[0] = 0xAA;
    data_to_send[1] = 0xAA;
    data_to_send[2] = 0xEF;
    data_to_send[3] = 2;
    data_to_send[4] = head;
    data_to_send[5] = check_sum;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < 6; i++)
        sum += data_to_send[i];
    data_to_send[6] = sum;

    ANO_DT_Send_Data(data_to_send, 7);
}

/////////////////////////////////////////////////////////////////////////////////////
// Data_Receive_Prepare������Э��Ԥ����������Э��ĸ�ʽ�����յ������ݽ���һ�θ�ʽ�Խ�������ʽ��ȷ�Ļ��ٽ������ݽ���
// ��ֲʱ���˺���Ӧ���û���������ʹ�õ�ͨ�ŷ�ʽ���е��ã����紮��ÿ�յ�һ�ֽ����ݣ�����ô˺���һ��
// �˺������������ϸ�ʽ������֡�󣬻����е������ݽ�������
void ANO_DT_Data_Receive_Prepare(uint8_t data)
{
    static uint8_t RxBuffer[50];
    static uint8_t _data_len = 0, _data_cnt = 0;
    static uint8_t state = 0;

    if (state == 0 && data == 0xAA)
    {
        state = 1;
        RxBuffer[0] = data;
    }
    else if (state == 1 && data == 0xAF)
    {
        state = 2;
        RxBuffer[1] = data;
    }
    else if (state == 2 && data < 0XF1)
    {
        state = 3;
        RxBuffer[2] = data;
    }
    else if (state == 3 && data < 50)
    {
        state = 4;
        RxBuffer[3] = data;
        _data_len = data;
        _data_cnt = 0;
    }
    else if (state == 4 && _data_len > 0)
    {
        _data_len--;
        RxBuffer[4 + _data_cnt++] = data;
        if (_data_len == 0)
            state = 5;
    }
    else if (state == 5)
    {
        state = 0;
        RxBuffer[4 + _data_cnt] = data;
        ANO_DT_Data_Receive_Anl(RxBuffer, _data_cnt + 5);
    }
    else
        state = 0;
}
/////////////////////////////////////////////////////////////////////////////////////
// Data_Receive_Anl������Э�����ݽ������������������Ƿ���Э���ʽ��һ������֡���ú��������ȶ�Э�����ݽ���У��
// У��ͨ��������ݽ��н�����ʵ����Ӧ����
// �˺������Բ����û����е��ã��ɺ���Data_Receive_Prepare�Զ�����
void ANO_DT_Data_Receive_Anl(uint8_t *data_buf, uint8_t num)
{
    uint8_t sum = 0;
    for (uint8_t i = 0; i < (num - 1); i++)
        sum += *(data_buf + i);
    if (!(sum == *(data_buf + num - 1)))
        return; // �ж�sum
    if (!(*(data_buf) == 0xAA && *(data_buf + 1) == 0xAF))
        return; // �ж�֡ͷ

    if (*(data_buf + 2) == 0X01)
    {
        // if (*(data_buf + 4) == 0X01)
        //     mpu6050.Acc_CALIBRATE = 1;
        // if (*(data_buf + 4) == 0X02)
        //     mpu6050.Gyro_CALIBRATE = 1;
        // if (*(data_buf + 4) == 0X03)
        // {
        //     mpu6050.Acc_CALIBRATE = 1;
        //     mpu6050.Gyro_CALIBRATE = 1;
        // }
    }

    if (*(data_buf + 2) == 0X02)
    {
        if (*(data_buf + 4) == 0X01)
        {
            f.send_pid1 = 1;
            f.send_pid2 = 1;
            f.send_pid3 = 1;
            f.send_pid4 = 1;
            f.send_pid5 = 1;
            f.send_pid6 = 1;
        }
        if (*(data_buf + 4) == 0X02)
        {
        }
        if (*(data_buf + 4) == 0XA0) // ��ȡ�汾��Ϣ
        {
            f.send_version = 1;
        }
        if (*(data_buf + 4) == 0XA1) // �ָ�Ĭ�ϲ���
        {
            Para_ResetToFactorySetup();
        }
    }

    if (*(data_buf + 2) == 0X10) // PID1
    {
        // ctrl_1.PID[PIDROLL].kp = 0.001 * ((int16_t)(*(data_buf + 4) << 8) | *(data_buf + 5));
        // ctrl_1.PID[PIDROLL].ki = 0.001 * ((int16_t)(*(data_buf + 6) << 8) | *(data_buf + 7));
        // ctrl_1.PID[PIDROLL].kd = 0.001 * ((int16_t)(*(data_buf + 8) << 8) | *(data_buf + 9));
        // ctrl_1.PID[PIDPITCH].kp = 0.001 * ((int16_t)(*(data_buf + 10) << 8) | *(data_buf + 11));
        // ctrl_1.PID[PIDPITCH].ki = 0.001 * ((int16_t)(*(data_buf + 12) << 8) | *(data_buf + 13));
        // ctrl_1.PID[PIDPITCH].kd = 0.001 * ((int16_t)(*(data_buf + 14) << 8) | *(data_buf + 15));
        // ctrl_1.PID[PIDYAW].kp = 0.001 * ((int16_t)(*(data_buf + 16) << 8) | *(data_buf + 17));
        // ctrl_1.PID[PIDYAW].ki = 0.001 * ((int16_t)(*(data_buf + 18) << 8) | *(data_buf + 19));
        // ctrl_1.PID[PIDYAW].kd = 0.001 * ((int16_t)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
        Param_SavePID();
    }
    if (*(data_buf + 2) == 0X11) // PID2
    {
        // ctrl_1.PID[PID4].kp = 0.001 * ((int16_t)(*(data_buf + 4) << 8) | *(data_buf + 5));
        // ctrl_1.PID[PID4].ki = 0.001 * ((int16_t)(*(data_buf + 6) << 8) | *(data_buf + 7));
        // ctrl_1.PID[PID4].kd = 0.001 * ((int16_t)(*(data_buf + 8) << 8) | *(data_buf + 9));
        // ctrl_1.PID[PID5].kp = 0.001 * ((int16_t)(*(data_buf + 10) << 8) | *(data_buf + 11));
        // ctrl_1.PID[PID5].ki = 0.001 * ((int16_t)(*(data_buf + 12) << 8) | *(data_buf + 13));
        // ctrl_1.PID[PID5].kd = 0.001 * ((int16_t)(*(data_buf + 14) << 8) | *(data_buf + 15));
        // ctrl_1.PID[PID6].kp = 0.001 * ((int16_t)(*(data_buf + 16) << 8) | *(data_buf + 17));
        // ctrl_1.PID[PID6].ki = 0.001 * ((int16_t)(*(data_buf + 18) << 8) | *(data_buf + 19));
        // ctrl_1.PID[PID6].kd = 0.001 * ((int16_t)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
        Param_SavePID();
    }
    if (*(data_buf + 2) == 0X12) // PID3
    {
        // ctrl_2.PID[PIDROLL].kp = 0.001 * ((int16_t)(*(data_buf + 4) << 8) | *(data_buf + 5));
        // ctrl_2.PID[PIDROLL].ki = 0.001 * ((int16_t)(*(data_buf + 6) << 8) | *(data_buf + 7));
        // ctrl_2.PID[PIDROLL].kd = 0.001 * ((int16_t)(*(data_buf + 8) << 8) | *(data_buf + 9));
        // ctrl_2.PID[PIDPITCH].kp = 0.001 * ((int16_t)(*(data_buf + 10) << 8) | *(data_buf + 11));
        // ctrl_2.PID[PIDPITCH].ki = 0.001 * ((int16_t)(*(data_buf + 12) << 8) | *(data_buf + 13));
        // ctrl_2.PID[PIDPITCH].kd = 0.001 * ((int16_t)(*(data_buf + 14) << 8) | *(data_buf + 15));
        // ctrl_2.PID[PIDYAW].kp = 0.001 * ((int16_t)(*(data_buf + 16) << 8) | *(data_buf + 17));
        // ctrl_2.PID[PIDYAW].ki = 0.001 * ((int16_t)(*(data_buf + 18) << 8) | *(data_buf + 19));
        // ctrl_2.PID[PIDYAW].kd = 0.001 * ((int16_t)(*(data_buf + 20) << 8) | *(data_buf + 21));
        ANO_DT_Send_Check(*(data_buf + 2), sum);
        Param_SavePID();
    }
    if (*(data_buf + 2) == 0X13) // PID4
    {
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    if (*(data_buf + 2) == 0X14) // PID5
    {
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
    if (*(data_buf + 2) == 0X15) // PID6
    {
        ANO_DT_Send_Check(*(data_buf + 2), sum);
    }
}

void ANO_DT_Send_Version(uint8_t hardware_type, uint16_t hardware_ver, uint16_t software_ver, uint16_t protocol_ver, uint16_t bootloader_ver)
{
    uint8_t _cnt = 0;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0;

    data_to_send[_cnt++] = hardware_type;
    data_to_send[_cnt++] = BYTE1(hardware_ver);
    data_to_send[_cnt++] = BYTE0(hardware_ver);
    data_to_send[_cnt++] = BYTE1(software_ver);
    data_to_send[_cnt++] = BYTE0(software_ver);
    data_to_send[_cnt++] = BYTE1(protocol_ver);
    data_to_send[_cnt++] = BYTE0(protocol_ver);
    data_to_send[_cnt++] = BYTE1(bootloader_ver);
    data_to_send[_cnt++] = BYTE0(bootloader_ver);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, int32_t alt, uint8_t fly_model, uint8_t armed)
{
    uint8_t _cnt = 0;
    int16_t _temp;
    int32_t _temp2 = alt;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x01;
    data_to_send[_cnt++] = 0;

    _temp = (int)(angle_rol * 100);
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = (int)(angle_pit * 100);
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = (int)(angle_yaw * 100);
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    data_to_send[_cnt++] = BYTE3(_temp2);
    data_to_send[_cnt++] = BYTE2(_temp2);
    data_to_send[_cnt++] = BYTE1(_temp2);
    data_to_send[_cnt++] = BYTE0(_temp2);

    data_to_send[_cnt++] = fly_model;

    data_to_send[_cnt++] = armed;

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Senser(int16_t a_x, int16_t a_y, int16_t a_z, int16_t g_x, int16_t g_y, int16_t g_z, int16_t m_x, int16_t m_y, int16_t m_z, int32_t bar)
{
    uint8_t _cnt = 0;
    int16_t _temp;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x02;
    data_to_send[_cnt++] = 0;

    _temp = a_x;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = a_y;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = a_z;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    _temp = g_x;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = g_y;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = g_z;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    _temp = m_x;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = m_y;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = m_z;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_RCData(uint16_t thr, uint16_t yaw, uint16_t rol, uint16_t pit, uint16_t aux1, uint16_t aux2, uint16_t aux3, uint16_t aux4, uint16_t aux5, uint16_t aux6)
{
    uint8_t _cnt = 0;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x03;
    data_to_send[_cnt++] = 0;
    data_to_send[_cnt++] = BYTE1(thr);
    data_to_send[_cnt++] = BYTE0(thr);
    data_to_send[_cnt++] = BYTE1(yaw);
    data_to_send[_cnt++] = BYTE0(yaw);
    data_to_send[_cnt++] = BYTE1(rol);
    data_to_send[_cnt++] = BYTE0(rol);
    data_to_send[_cnt++] = BYTE1(pit);
    data_to_send[_cnt++] = BYTE0(pit);
    data_to_send[_cnt++] = BYTE1(aux1);
    data_to_send[_cnt++] = BYTE0(aux1);
    data_to_send[_cnt++] = BYTE1(aux2);
    data_to_send[_cnt++] = BYTE0(aux2);
    data_to_send[_cnt++] = BYTE1(aux3);
    data_to_send[_cnt++] = BYTE0(aux3);
    data_to_send[_cnt++] = BYTE1(aux4);
    data_to_send[_cnt++] = BYTE0(aux4);
    data_to_send[_cnt++] = BYTE1(aux5);
    data_to_send[_cnt++] = BYTE0(aux5);
    data_to_send[_cnt++] = BYTE1(aux6);
    data_to_send[_cnt++] = BYTE0(aux6);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];

    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Power(uint16_t votage, uint16_t current)
{
    uint8_t _cnt = 0;
    uint16_t temp;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x05;
    data_to_send[_cnt++] = 0;

    temp = votage;
    data_to_send[_cnt++] = BYTE1(temp);
    data_to_send[_cnt++] = BYTE0(temp);
    temp = current;
    data_to_send[_cnt++] = BYTE1(temp);
    data_to_send[_cnt++] = BYTE0(temp);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];

    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_MotoPWM(uint16_t m_1, uint16_t m_2, uint16_t m_3, uint16_t m_4, uint16_t m_5, uint16_t m_6, uint16_t m_7, uint16_t m_8)
{
    uint8_t _cnt = 0;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x06;
    data_to_send[_cnt++] = 0;

    data_to_send[_cnt++] = BYTE1(m_1);
    data_to_send[_cnt++] = BYTE0(m_1);
    data_to_send[_cnt++] = BYTE1(m_2);
    data_to_send[_cnt++] = BYTE0(m_2);
    data_to_send[_cnt++] = BYTE1(m_3);
    data_to_send[_cnt++] = BYTE0(m_3);
    data_to_send[_cnt++] = BYTE1(m_4);
    data_to_send[_cnt++] = BYTE0(m_4);
    data_to_send[_cnt++] = BYTE1(m_5);
    data_to_send[_cnt++] = BYTE0(m_5);
    data_to_send[_cnt++] = BYTE1(m_6);
    data_to_send[_cnt++] = BYTE0(m_6);
    data_to_send[_cnt++] = BYTE1(m_7);
    data_to_send[_cnt++] = BYTE0(m_7);
    data_to_send[_cnt++] = BYTE1(m_8);
    data_to_send[_cnt++] = BYTE0(m_8);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];

    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_PID(uint8_t group, float p1_p, float p1_i, float p1_d, float p2_p, float p2_i, float p2_d, float p3_p, float p3_i, float p3_d)
{
    uint8_t _cnt = 0;
    int16_t _temp;

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0x10 + group - 1;
    data_to_send[_cnt++] = 0;

    _temp = p1_p * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p1_i * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p1_d * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_p * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_i * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p2_d * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_p * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_i * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);
    _temp = p3_d * 1000;
    data_to_send[_cnt++] = BYTE1(_temp);
    data_to_send[_cnt++] = BYTE0(_temp);

    data_to_send[3] = _cnt - 4;

    uint8_t sum = 0;
    for (uint8_t i = 0; i < _cnt; i++)
        sum += data_to_send[i];

    data_to_send[_cnt++] = sum;

    ANO_DT_Send_Data(data_to_send, _cnt);
}
void ano_dt_task(void *p)
{
    while (1)
    {
        if(ant_run)
        {
            ANO_DT_Data_Exchange();
            rt_thread_mdelay(1);
        }
        else
        {
            rt_thread_mdelay(1000);
        }
    }
}
int ano_dt_task_init()
{
    rt_thread_t tid;
    tid = rt_thread_create("ano_dt",
                           ano_dt_task,
                           RT_NULL,
                           2048,
                           10,
                           20);
    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
        LOG_I("task startup");
    }
    else
    {
        LOG_E("task startup fail");
        return -RT_ENOMEM;
    }
    return RT_EOK;
}
void ano(int argc, char *agrv[])
{
    if(argc == 2)
    {
        if(rt_strcmp(agrv[1],"1") == 0)
        {
            ant_run = 1;
        }
        else if(rt_strcmp(agrv[1],"0") == 0)
        {
            ant_run = 0;
        }
        else if(rt_strcmp(agrv[1],"on") == 0)
        {
            ant_run = 1;
        }
        else if(rt_strcmp(agrv[1],"off"))
        {
            ant_run = 0;
        }
    }
}
// MSH_CMD_EXPORT(ano, ano on/off);
// ZINO_APP_EXPORT(ano_dt_task_init);
/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/
